#include "read_data.h"

using namespace sad;

static double ns_to_second = 0.000000001;

read_data::read_data(const std::string& path) {
    path_ = path;
    origin_timestamp = 0.0;
}

std::vector<std::string> read_data::split(std::string str, std::string pattern) {
	std::string::size_type pos;
	std::vector<std::string> result;
	str += pattern;
	int size = str.size();

	for (int i = 0; i < size; i++) {
		pos = str.find(pattern, i);

		if (pos < size) {
			std::string s = str.substr(i, pos - i);
			result.push_back(s);
			i = pos + pattern.size() - 1;
		}
	}
	return result;
}

bool read_data::LoadSensorData(const std::string& encoder_file_path, std::unordered_map<std::string, std::string>* time_data_map) {
    std::ifstream encoder_file(encoder_file_path);
    if (!encoder_file.is_open()) {
        std::cout << "[LoadSensorData]: Failed to open encoder file." << std::endl;
        return false;
    } 

    std::string line_str, time_str;
    while (std::getline(encoder_file, line_str)) {
        std::stringstream ss(line_str);
        if (!std::getline(ss, time_str, ',')) {
            std::cout << "[LoadSensorData]: Find a bad line in the encoder file.: " << line_str << std::endl;
            return false;
        }

        time_data_map->emplace(time_str, line_str);
    }

    return true;
}

bool read_data::read_only_kaist_data(std::unordered_map<std::string, std::string>& time_encoder_map, 
                              		 std::unordered_map<std::string, std::string>& time_imu_map,
                              		 std::unordered_map<std::string, std::string>& time_gps_map, 
                              		 std::unordered_map<std::string, std::string>& time_vrs_gps_map,
                                     std::unordered_map<std::string, std::string>& time_sick_middle_map,
                                     std::unordered_map<std::string, std::string>& time_velodyne_left_map) {
	bool state = true;
	// Load all encoder data to buffer.
    if (!LoadSensorData(path_ + "sensor_data/encoder.csv", &time_encoder_map)) {
        std::cout << "[main]: Failed to load encoder data." << std::endl;
        state = false;
    } 

    // Load all imu date to buffer.
    if (!LoadSensorData(path_ + "sensor_data/xsens_imu.csv", &time_imu_map)) {
        std::cout << "[main]: Failed to load imu data." << std::endl;
        state = false;
    } 

    // Load all gps data to buffer.
    if (!LoadSensorData(path_ + "sensor_data/gps.csv", &time_gps_map)) {
        std::cout << "[main]: Failed to load gps data." << std::endl;
        state = false;
    } 

    if (!LoadSensorData(path_ + "sensor_data/vrs_gps.csv", &time_vrs_gps_map)) {
        std::cout << "[main]: Failed to load gps data." << std::endl;
        state = false;
    } 	

    if (!LoadSensorData(path_ + "sensor_data/SICK_middle_stamp.csv", &time_sick_middle_map)) {
        std::cout << "[main]: Failed to load SICK_middle data." << std::endl;
        state = false;
    }  

    if (!LoadSensorData(path_ + "sensor_data/VLP_left_stamp.csv", &time_velodyne_left_map)) {
        std::cout << "[main]: Failed to load VLP_left_stamp data." << std::endl;
        state = false;
    }  
    
    std::ifstream file_data_stamp(path_ + "sensor_data/data_stamp.csv");
    if (!file_data_stamp.is_open()) {
        std::cout << "[main]: Failed to open data_stamp data." << std::endl;
    }

    std::vector<std::string> line_data_vec;
    line_data_vec.reserve(3);
    std::string line_str, value_str;

	std::vector<double> times;

    while (std::getline(file_data_stamp, line_str)) {
        line_data_vec.clear();
        std::stringstream ss(line_str);
        while (std::getline(ss, value_str, ',')) { line_data_vec.push_back(value_str); }

        const double kToSecond = 1e-9;
        const std::string& time_str = line_data_vec[0];

        const double timestamp = std::stod(time_str) * kToSecond;
    	times.push_back(timestamp);
    	if(times.size() > 200) {
    		break;
    	}
    }

    std::sort(times.begin(),times.end());
    
    origin_timestamp = times[0] - 10.0;

    return state;
}